/*
 * STUDUINO library
 * (C) 2014 Artec Corporation
 * 
 * In order to run this program in Arduino IDE, the Studuino library needs to be installed no this system.
 * 
 * Refer to the website below for information on how to install the Studuino library.
 * http://www.artec-kk.co.jp/studuino
 */
#define DEF_I2C_GYRO
#define DEF_I2C_ACCEL
#include <Arduino.h>
#include <Servo.h>
#include <Wire.h>
#include <MMA8653.h>
#include <MPU6050.h>
#include "Studuino.h"
// **********************************************************************
// Declaration
// **********************************************************************
// List of elements
struct cell_t{
  struct cell_t* next;
  float data;
};
typedef cell_t cell;
// **********************************************************************
// Definition
// **********************************************************************
#define DCMPWR(power)	((byte)(min(max(0, ((float)(power) * 2.55)),255)))

#define BMIN    (0)     // The minumum value of the sensor in the Block programming environment.
#define BMAX    (100)   // The maximum value of the sensor in the Block programming environment.
#define ANAMIN  (0)     // The minimum value of the analog sensor on the Studuino board.
#define ANAMAX  (1023)  // The maximum value of the analog sensor on the Studuino board.
#define ACCMIN  (-128)  // The minimum value of the accelerometer on the Studuino board.
#define ACCMAX  (127)   // The maximum value of the accelerometer on the Studuino board.
#define PUSHSWITCH(port)       (board.GetPushSwitchValue(port))
#define TOUCH_SENSOR(port)     (board.GetTouchSensorValue(port))
#define LIGHT_SENSOR(port)     (map(board.GetLightSensorValue(port), ANAMIN, ANAMAX, BMIN, BMAX))
#define SOUND_SENSOR(port)     (map(board.GetSoundSensorValue(port), ANAMIN, ANAMAX, BMIN, BMAX))
#define IRPHOTOREFLECTOR(port) (map(board.GetIRPhotoreflectorValue(port), ANAMIN, ANAMAX, BMIN, BMAX))
#define ACCELEROMETER(axis)    (map(board.GetAccelerometerValue(axis), ACCMIN, ACCMAX, BMIN, BMAX))

#define TEMPERATURE_SENSOR(port)    (GetTemperatureSensorValue(port))
#define GYRO_SENSOR(port)    (GetGyroscopeValue(port))
#define ULTRASONIC_SENSOR()    (GetUltrasonicSensorValue())

#define DWEIGHT	(1.818)

#define MINMAX(v, mv, Mv)	(min(max(v,mv),Mv))

const byte SQRT  =  (0);   // √n
const byte ABS   =  (1);   // |n|
const byte SIN   =  (2);   // sin(n)
const byte COS   =  (3);   // cos(n)
const byte TAN   =  (4);   // tan(n)
const byte LN    =  (5);   // loge
const byte LOG   =  (6);   // log10
const byte POWE  =  (7);   // e^
const byte POW10 =  (8);   // 10^

const word BTONE[] = {
  BZR_C3,  BZR_CS3, BZR_D3,  BZR_DS3, BZR_E3,  BZR_F3,  BZR_FS3, BZR_G3,  BZR_GS3, BZR_A3,  BZR_AS3, BZR_B3,  
  BZR_C4,  BZR_CS4, BZR_D4,  BZR_DS4, BZR_E4,  BZR_F4,  BZR_FS4, BZR_G4,  BZR_GS4, BZR_A4,  BZR_AS4, BZR_B4,  
  BZR_C5,  BZR_CS5, BZR_D5,  BZR_DS5, BZR_E5,  BZR_F5,  BZR_FS5, BZR_G5,  BZR_GS5, BZR_A5,  BZR_AS5, BZR_B5,  
  BZR_C6,  BZR_CS6, BZR_D6,  BZR_DS6, BZR_E6,  BZR_F6,  BZR_FS6, BZR_G6,  BZR_GS6, BZR_A6,  BZR_AS6, BZR_B6,  
  BZR_C7,  BZR_CS7, BZR_D7,  BZR_DS7, BZR_E7,  BZR_F7,  BZR_FS7, BZR_G7,  BZR_GS7, BZR_A7,  BZR_AS7, BZR_B7,  
  BZR_C8,  
};
#define TONENUM	((sizeof(BTONE)/sizeof(word))-1)
#define BHZ(num)  (BTONE[(byte)(min(max(0, (num-48)),TONENUM))])

// for Servomotor calibration
const byte SVCD2  = (4);
const byte SVCD4  = (5);
const byte SVCD7  = (6);
const byte SVCD8  = (7);
const byte SVCD9  = (0);
const byte SVCD10 = (1);
const byte SVCD11 = (2);
const byte SVCD12 = (3);
// Min/Max of servomotor's degree
#define SVRANGE(deg)	(min(max(0, (deg)), 180))
#define SYNCSVRANGE(dly)	(min(max(0, (dly)), 20))

// **********************************************************************
// Prototype declaration
// **********************************************************************
// List
int listDelete(struct cell_t* p, int pos);              // Delete element
int listAdd(struct cell_t* p, float data);              // Add element
int listInsert(struct cell_t *p, int pos, float data);  // Insert element
int listReplace(struct cell_t *p, int pos, float data); // Replace element
int listLength(struct cell_t *p);                       // Get list length
float listItem(struct cell_t *p, int pos);              // Get element
bool listIsContain(struct cell_t *p, float data);       // Does the list contain the data?
// Round process
int scratchRound(float arg);
// Mathematics
float math(byte opeID, float arg);              // Arithmetic operation

void resetTimer();
float getTimer();

float GetTemperatureSensorValue(byte connector);
double GetGyroscopeValue(byte connector);
float GetUltrasonicSensorValue();

// Robot setup process
void artecRobotSetup();
// Robot main process
void artecRobotMain();

// **********************************************************************
// Global variable
// **********************************************************************
Studuino board;	// Objects on the Studuino board
unsigned long StartTime;	// For timer

bool IRRemoteUsed = false;	// 赤外線受信センサ使用中フラグ
bool BeepOn = false;
bool DCMotorOn = false;
// **********************************************************************
// Program
// **********************************************************************
